Copyright □ 1999, by Professor Jong-Hwan Kim, Korea Advanced Institute of Science and Technology (KAIST).  All rights reserved. Reproduction or translation of any part of this work is unlawful. Request for permission or further information should be addressed to Professor Jong-Hwan Kim at <johkim@vivaldi.kaist.ac.kr> or <Tel: +82-42-869-3448>.

Prof. Jong-Hwan Kim	Intelligent Control Lab, KAIST

8.로봇 제어 프로그램 Ⅱ

이번 주에는 로봇을 원하는 자세로 이동하게 하는 Posture 함수에 대해서 설명한다. 로봇의 자세는 X-Y 좌표계에서 위치(x,y)와 로봇이 향하는 방향각([eqn: theta{}])을 의미한다. 로봇축구에서는 단순한 위치 제어뿐만 아니라 목적지에서 원하는 방향각을 제어함으로써 정확한 슛을 할 수 있다.

8.1 Posture 함수

Posture 함수는 로봇을 원하는 자세(위치와 방향각)로 이동시키는 함수이다. 즉, 원하는 자세([eqn: x_d , y_d , theta_d])와 현재 로봇의 자세([eqn: x_r , y_r , theta_r])와의 오차를 측정하여 로봇을 원하는 자세로 이동하게 한다.

그림 8. 자세 제어

그림 8.1에서 살펴보면 원하는 위치와의 거리 오차가 [eqn: d_e]이고, 각도 오차는 [eqn: theta_e]로 다음과 같이 주어진다.

[eqn: &d_e = &sqrt{d_x ^2 + d_y ^2~}# &theta_e = &theta_d - theta_r]

(8.1)

여기서 [eqn: d_x]는 x좌표에서 원하는 위치와 로봇과의 거리차이고, [eqn: d_y]는 y좌표에서 원하는 위치와 로봇과의 거리차이며, [eqn: theta_d]는 목적지에서 원하는 방향각이고 [eqn: theta_r]은 현재 로봇의 방향각이다.

이 두 오차를 줄이기 위해 로봇의 오른쪽 바퀴와 왼쪽 바퀴의 속도 입력을 간단한 비례(Proportional)제어를 사용하여 다음과 같이 구한다:

[eqn: &V_L = &K_P_distance CDOT d_e - &K_P_\angle CDOT theta_e # &V_R = &K_P_distance CDOT d_e + &K_P_\angle CDOT theta_e]

(8.2)

여기서 [eqn: K_{P_distance}와 ~K_{P_\angle}]는 각각 거리오차 및 각도오차에 곱해지는 상수 값이다. 더 정밀한 제어를 위해서는 위의 속도제어 입력에 거리오차와 각도오차의 시간에 대한 변화량,[eqn: dot d_e 와 ~dot theta_e ,]을 추가할 수 있다. 이를 PD(Proportional-Derivative)제어기라 한다. 식 (8.2)를 이용하여 왼쪽 바퀴와 오른쪽 바퀴의 속도 입력을 주면 로봇은 원하는 자세로 접근하여 멈추게 된다. 하지만 실제 로봇 축구에서는 로봇은 계속 공을 차야 하므로 공에 가까워졌을 때 속도가 0이 되는 것은 피해야 한다. 이에 관하여는 나중에 다시 다루고자한다.

다음은 식 (8.2)를 기준으로 만든 Posture (자세) 함수 코드이다. 로봇의 x, y 좌표는 각각 PositonOfHomeRobot[whichrobot][0]과 PositionOfHomeRobot[whichrobot][1]에서 얻을 수 있다. 여기서 whichrobot은 로봇을 구별하기 위한 로봇의 번호를 나타내며, 로봇이 세대이므로 0, 1, 2의 값을 가진다. 로봇의 방향각은  AngleOfHomeRobot에서 얻을 수 있다.

void Posture(int whichrobot, double x, double y)

{

int desired_angle,theta_e, vl, vr;	// [eqn: theta_d , theta_e] 변수 선언

double dx,dy,distance_e, Kd=1.0, Ka=0.1;	//[eqn: {rm Kd} = K_{P_distance}, ~{rm Ka}= K_{P_\angle}]

dx = x-PositionOfHomeRobot[whichrobot][0];	// x축으로의 거리차

dy = y-PositionOfHomeRobot[whichrobot][1];	// y축으로의 거리차

distance_e = sqrt(dx*dx+dy*dy);	// 원하는 위치와 로봇과의 거리차

// 적절한 제어기 계수를 설정한다.

if(distance_e > 100)	Kd = 1.0, Ka = 0.12;	// 거리차가 100cm 이상일 때

else if(distance_e > 50)	Kd = 1.2, Ka = 0.125;	// 거리차가 50cm 이상일 때

else if(distance_e > 30)	Kd = 1.4, Ka = 0.13;	// 거리차가 30cm 이상일 때

else if(distance_e > 20)	Kd = 2.0, Ka = 0.14;	// 거리차가 20cm 이상일 때

else if(distance_e > 10)	Kd = 3.0, Ka = 0.16;	// 거리차가 10cm 이상일 때

else	Kd = 5.0, Ka = 0.18;	// 거리차가 10cm 이내일 때

if(dx==0 && dy==0)	desired_angle = 90;	// dx = 0으로 나눠지는 것을 방지

// x, y 거리 오차에서 원하는 각도 구함

else	desired_angle = (int)(180/M_PI*atan2((double)(dy),(double)(dx)));

theta_e = desired_angle-AngleOfHomeRobot[whichrobot];	// [eqn: theta_e = theta_d - theta_r]

while(theta_e > 180)	theta_e -= 360;	// [eqn: theta_e] 를 [eqn: -180 DEG sim 180 DEG]까지 제한

while(theta_e < -180)	theta_e += 360;

if(theta_e < -90){	// 로봇의 방향이 앞뒤로 바뀌는 것을 고려

theta_e += 180;

distance_e = -distance_e;

}

else if(theta_e > 90){	// 로봇의 방향이 앞뒤로 바뀌는 것을 고려

theta_e -= 180;

distance_e = -distance_e;

}

vl = (int)(Kd*distance_e - Ka*(double)theta_e);	// 왼쪽 바퀴의 제어입력 속도

vr = (int)(Kd*distance_e + Ka*(double)theta_e);	// 오른쪽 바퀴의 제어입력 속도

Velocity(whichrobot,vl,vr);	// 계산된 속도 데이터로 Velocity 함수를 호출

}

위의 프로그램은 원하는 자세로 로봇을 움직이게 하는 기본함수이다. 그러나 앞에서도 언급하였듯이 다음의 두 가지 단점이 있다. 하나는, 로봇이 원하는 위치로 접근함에 따라 로봇의 속도가 점점 줄어들어 결국 멈추게 된다. 즉, 원하는 위치가 공의 좌표일 때에는 공 근처에 도달했을 때 멈추게 된다. 또 다른 문제점은 그림 8.2에 나타내었듯이 [eqn: theta_e]가 [eqn: 90 DEG, -90DEG]를 기준으로 로봇의 앞뒤가 변하므로 원하는 위치와 로봇이 이루는 각이 [eqn: 90 DEG, -90DEG]에서 흔들림 현상이 생긴다.

(a)로봇이 오른쪽으로	(b) 로봇이 왼쪽으로

이동하다 왼쪽으로           이동하다 오른쪽으로

방향을 바꿀 때              방향을 바꿀 때

그림 8.2 Posture 함수의 흔들림 현상

즉, 그림 8.2(a)는 로봇이 공 쪽으로 이동하기 위해서 오른쪽으로 움직이다가 [eqn: theta_e]가 90[eqn: DEG]보다 커져서 왼쪽으로 진행 방향을 바꾸는 경우이다. 로봇의 진행방향을 바꾸고 왼쪽으로 이동하다가 다시 [eqn: theta_e]가 -90[eqn: DEG]보다 작아져서 진행방향을 오른쪽으로 바꾸는 경우가 그림 8.2(b)이다. 이처럼 로봇은 공을 두고서 공 근처로 이동하기 전에 진동을 반복할 수가 있다.

이러한 문제점을 해결하기 위하여 다음 프로그램에서는 로봇 중심에서의 속도 [eqn: v_c]를 일정한 PWM 신호 속도 70 (PWM신호: 양의 방향 128[eqn: sim]255, 음의 방향 0[eqn: sim]126)으로 고정했다. 다음의 관계식은 로봇 중심에서의 속도 [eqn: v_c ,] 각속도 [eqn: omega]와 로봇의 오른쪽과 왼쪽 바퀴의 속도와의 관계를 나타낸다.

[eqn: v_c = {v_R + v_L}over{2}, &omega = {v_R - v_L}over{D}]   여기서 D는 로봇의 양바퀴 사이의 거리이다.

(8.2)

위 식으로부터 오른쪽과 왼쪽 바퀴의 속도를 구하면 다음과 같다.

[eqn: v_R = v_c + D over 2 omega, &v_L = v_c - D over 2 omega]

(8.3)

[eqn: omega = - dot theta_e]이고 정확히 표현을 하면 [eqn: omega = - {(theta_e (k) - theta_e (k-1))}over {T}]이다. 여기서 k와 k-1은 단계수(step number)를, T는 샘플링(sampling) 시간을 나타낸다. 하지만 계산을 간략히 하기 위해 실제 프로그램에서는[eqn: D over 2 omega]를 [eqn: {rm Ka} CDOT theta_e]로 구현했다. 또한, [eqn: v_c]는 로봇이 공에 가까이 접근했을 때 슛의 정밀도를 높이기 위해 지수함수를 사용하여 그 속도 값을 줄였다. 즉, 공과 멀리 떨어져 있을 때는 [eqn: v_c CDOT ( 1 over{1+ { e}^{-3 CDOT d_e } }-0.3)]에 의해 49 정도가 되고, 공과의 거리가 근접 거리인 1cm 정도가 되었을 때 [eqn: v_c]는 약 45 정도로 하였다. 이를 그림 8.3에 나타내었다. 그러나 이러한 값들은 설계자가 임의로 설정할 수 있다.

[eqn: v_c]

그림 8.3   의 거리에 따른 변화

다음은 위에서 언급된 점들을 고려하여 개선된 자세 함수이다.

void Posture(int whichrobot, double x, double y)

{

int desired_angle=0, theta_e = 0, d_angle=0, vl, vr, vc=70;	// 변수 선언, vc는 로봇중심에서의 속도

double dx, dy, d_e, Ka=10.0/90.0;

dx=x-PositionOfHomeRobot[whichrobot][0];

dy=y-PositionOfHomeRobot[whichrobot][1];

[eqn: v_c]

d_e=sqrt(dx*dx+dy*dy);	// 원하는 위치와의 거리차 측정

if(dx==0 && dy==0)	desired_angle = 90;	// [eqn: theta_d] 를 측정

else	desired_angle = (int)(180./M_PI*atan2((double)(dy),(double)(dx)));

theta_e = desired_angle-AngleOfHomeRobot[whichrobot];	// [eqn: theta_e = theta_d - theta_r]

while(theta_e > 180)	theta_e -= 360;	// [eqn: theta_e] 각도를 제한

while(theta_e < -180)	theta_e += 360;

if(d_e>100.)	Ka = 17./90.;	// 거리에 따른 각도 계수 설정

else if(d_e > 50)	Ka = 19./90.;

else if(d_e > 30)	Ka = 21./90.;

else if(d_e > 20)	Ka = 23./90.;

else	Ka = 25./90.;

if (theta_e > 95 || theta_e < -95) {

theta_e += 180;	// 로봇의 앞뒤를 바꿈

if (theta_e>180) theta_e -= 360;

if(theta_e>80)	theta_e = 80;	// [eqn: theta_e]의 크기를 제한

if(theta_e<-80)	theta_e = -80;

if(d_e<5.0 && abs(theta_e)<40)	Ka = 0.1;	// 거리가 가깝고 각도오차가 어느정도	있을 때 예외 처리

vr = (int)(-vc * ( 1.0/(1.0+exp(-3.0*d_e)) -0.3 ) + Ka*theta_e);	// 거리가 작아질 때 vc를 감소

vl = (int)(-vc * ( 1.0/(1.0+exp(-3.0*d_e)) -0.3 ) - Ka*theta_e);

}

else if(theta_e < 85 && theta_e > -85){

if(d_e<5.0 && abs(theta_e)<40)	Ka = 0.1;	// 거리가 가깝고 각도오차가 어느정도	있을 때 예외 처리

vr = (int)( vc * ( 1.0/(1.0+exp(-3.0*d_e)) -0.3)+ Ka*theta_e);

vl = (int)( vc * ( 1.0/(1.0+exp(-3.0*d_e)) -0.3)- Ka*theta_e);

}

else{

vr = (int)(+.17*theta_e);	// 공과 로봇이 이루는 각도가 90[eqn: DEG]근처일 때

vl = (int)(-.17*theta_e);

}

Velocity(whichrobot, vl, vr);

}

이와 같이하면 로봇이 공 근처에 도달해도 로봇중심 속도 [eqn: v_c]가 일정하게 유지되므로 앞에서 설명한 프로그램과는 달리 로봇중심 속도가 어느 이하 (45정도) 로 감소하지 않는다. 이렇게 함으로써 로봇이 공과 멀리 있을 때는 [eqn: v_c]를 크게 설정하여 공 근처로 빨리 접근하게 하고, 공과 근접함에 따라 [eqn: v_c]를 줄여 나감으로써 정확한 자세 제어를 할 수 있게된다. 이로써 어느 속도 이상으로 슛을 할 수 있으며 또한 슛의 정밀도를 높일 수 있다.

하지만 여기서 보여준 방법은 하나의 예제일 뿐이고 최선의 방법은 아니다. 위의 Posture 함수를 사용하여 로봇이 공에서 멀리 떨어져 있을 때는 로봇 중심 속도([eqn: v_c])를 더 크게 할 수 있고, 공에 접근함에 따라 속도를 더 줄일 수 있다. 그리고 공을 슛하기 좋은 조건에 로봇이 위치하면 로봇의 중심속도를 크게 하여 로봇이 강한 슛을 하게 할 수도 있다. 또한, 설계자가 위에서 나타난 단점들을 다른 방법으로 해결할 수 있을 뿐 아니라, 프로그램에 사용된 여러 변수 값들을 설계자의 의도에 맞게 설정할 수 있다.